#include"vld_imu_slam/map.h"

namespace vld_imu_slam{

void Map::insertKeyFrame(Frame::Ptr p_frame){
    lock_guard<mutex> lck(keyframe_mutex_);
    p_current_frame_ = p_frame;
    //按道理是新发现的关键帧才会才入的，但是，为了鲁棒性，还是需要进行判断
    if(keyframes_.find(p_frame->keyframe_id_)==keyframes_.end()){
        keyframes_.insert(make_pair(p_frame->keyframe_id_, p_frame));
        pc_pose_->push_back(p_frame->getFramePosition());
    } 
    p_backend_->lidarOdomBackendOpt(keyframes_,loopfactor_v_);
    updateLocalmap();
}

void Map::updateLocalmap(){
    vector<Frame::Ptr> active_keyframes;
    int count = 0;
    for(int i = keyframes_.size()-1;i>=0&&count<num_active_keyframes_;i--){
        count++;
        active_keyframes.push_back(keyframes_[i]);
    }

    corner_localmap_.reset(new pcl::PointCloud<PointType>);
    surf_localmap_.reset(new pcl::PointCloud<PointType>);
    pcl::PointCloud<PointType>::Ptr pc(new pcl::PointCloud<PointType>);
    for(auto& kf : active_keyframes){
        pcl::transformPointCloud(*kf->pc_corner_, *pc, SE3toEigenM(kf->getPose()));
        *corner_localmap_ = *corner_localmap_ + *pc;
        pcl::transformPointCloud(*kf->pc_surface_, *pc, SE3toEigenM(kf->getPose()));
        *surf_localmap_ = *surf_localmap_ + *pc;
    }
}

void Map::searchLocalFrame(Frame::Ptr p_frame,vector<int>& point_search_Idx,double radius){
    vector<float> point_search_dis;
    p_kdtree_pose_->setInputCloud(pc_pose_);
    p_kdtree_pose_->radiusSearch(p_frame->getFramePosition(), radius, point_search_Idx, point_search_dis);
}

pcl::PointCloud<PointType>::Ptr Map::getCornerLocalMap(){
    return corner_localmap_;
}

pcl::PointCloud<PointType>::Ptr Map::getSurfLocalMap(){
    return surf_localmap_;
}

pcl::PointCloud<PointType>::Ptr Map::getMap(){
    pcl::PointCloud<PointType>::Ptr map(new pcl::PointCloud<PointType>);
    pcl::PointCloud<PointType>::Ptr pc(new pcl::PointCloud<PointType>);
    for(auto& kf : keyframes_){
        pcl::transformPointCloud(*kf.second->pc_full_, *pc, SE3toEigenM(kf.second->getPose()));
        *map = *map + *pc;
    }
    pcDownSample(map,0.5);
    return map;
}

void Map::loopThread(){
    lock_guard<mutex> lck(keyframe_mutex_);
    if(pc_pose_->empty())
        return;
    vector<int> pose_search_idx;
    searchLocalFrame(keyframes_[keyframes_.size()-1],pose_search_idx,2);
    int idx_loopframe = -1;
    for(auto& idx:pose_search_idx){
        if(abs(keyframes_[idx]->time_stamp_-keyframes_[keyframes_.size()-1]->time_stamp_) > 100 ){
            idx_loopframe = idx;
            break;
        }
    }
    vector<int> idx_localmap;
    pcl::PointCloud<PointType>::Ptr pc_localmap(new pcl::PointCloud<PointType>);
    pcl::PointCloud<PointType>::Ptr pc(new pcl::PointCloud<PointType>);
    if(idx_loopframe>=0){
        if((idx_loopframe-1)>=0)
            idx_localmap.emplace_back(idx_loopframe-1);
        if((idx_loopframe+1)<keyframes_.size())
            idx_localmap.emplace_back(idx_loopframe+1);
        for(auto& idx:idx_localmap){
            pcl::transformPointCloud(*keyframes_[idx]->pc_full_, *pc, SE3toEigenM(keyframes_[idx]->getPose()));
            *pc_localmap = *pc_localmap + *pc;
        }
        pcl::transformPointCloud(*keyframes_[keyframes_.size()-1]->pc_full_, *pc, SE3toEigenM(keyframes_[keyframes_.size()-1]->getPose()));
        // ICP参数设置
        static pcl::IterativeClosestPoint<PointType, PointType> icp;
        icp.setMaxCorrespondenceDistance(2*2);
        icp.setMaximumIterations(100);
        icp.setTransformationEpsilon(1e-6);
        icp.setEuclideanFitnessEpsilon(1e-6);
        icp.setRANSACIterations(0);

        // scan-to-map，调用icp匹配
        icp.setInputSource(pc);
        icp.setInputTarget(pc_localmap);
        pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
        icp.align(*unused_result);

        // 未收敛，或者匹配不够好
        if (icp.hasConverged() == false || icp.getFitnessScore() > 0.3)
            return;

        Eigen::Matrix4f T_loopframe_cur = icp.getFinalTransformation();
        // T_loopframe_cur = T_loopframe_cur.inverse().eval();
        Eigen::Matrix4d T_d;
        T_d<<T_loopframe_cur(0,0),T_loopframe_cur(0,1),T_loopframe_cur(0,2),T_loopframe_cur(0,3),
                    T_loopframe_cur(1,0),T_loopframe_cur(1,1),T_loopframe_cur(1,2),T_loopframe_cur(1,3),
                    T_loopframe_cur(2,0),T_loopframe_cur(2,1),T_loopframe_cur(2,2),T_loopframe_cur(2,3),
                    T_loopframe_cur(3,0),T_loopframe_cur(3,1),T_loopframe_cur(3,2),T_loopframe_cur(3,3);
        Eigen::Quaterniond q_d(T_d.block<3,3>(0,0));
        q_d.normalized();
        SE3 T_se3(q_d,Eigen::Vector3d(T_loopframe_cur(0,3),T_loopframe_cur(1,3),T_loopframe_cur(2,3)));
        // gtsam::Pose3 transform(T_loopframe_cur);
        LoopFactor loop_factor;
        loop_factor.cur_idx = keyframes_.size()-1;
        loop_factor.loop_idx = idx_loopframe;
        loop_factor.T_cur_loopframe = SE3togtsamPose(T_se3.inverse());
        gtsam::Vector Vector6(6);
        float noiseScore = icp.getFitnessScore();
        Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
        loop_factor.constraintNoise = gtsam::noiseModel::Diagonal::Variances(Vector6);
        loopfactor_v_.emplace_back(loop_factor);
    }
}

}
